#include "display.h"
#include "drv_rgb_led.h"
#include "drv_us015.h"
#include "main.h"

static uint16_t tick1ms;
static uint16_t tick500ms_ledState=0;
static uint8_t ledState=0;
static float distance_mm_now;
static float distance_mm;
static uint32_t servo_pwm_duty0;

void app_init(void)
{

}

void app_main_function(void)
{
    float tmp;
    tick1ms++;

    if(tick1ms>500){
        tick1ms=0;
        tick500ms_ledState++;
    }

    distance_mm_now = drv_us015_get_distance_mm(US015_CH0);
    tmp=distance_mm_now-distance_mm;
    
    if(tmp > 10 || tmp <(-10)){
        distance_mm = distance_mm_now;
        drv_rgb_led_SetColorByDistance(RGB_LED_0,distance_mm);

        distance_mm_now/=MAX_DISTANCE_MM;
        distance_mm_now*=128;
        display_draw_pulse(4,(uint8_t)distance_mm_now);
    }
    /*else{
        display_draw_pulse(0,128);
    }*/

    switch (ledState){
        case 0:/*G ON*/
        LL_GPIO_SetOutputPin(LED_RED_GPIO_Port, LED_RED_Pin);
        LL_GPIO_SetOutputPin(LED_YELLOW_GPIO_Port, LED_YELLOW_Pin);
        LL_GPIO_ResetOutputPin(LED_GREEN_GPIO_Port, LED_GREEN_Pin);
        LL_TIM_OC_SetCompareCH1(TIM3,1100);
        ledState=1;
        break;
        case 1:/*G ON IDLE,state keep*/
        if(distance_mm<8){
            ledState = 2;
            tick500ms_ledState=0;
        }
        break;
        case 2:/*Wait stable for at least 1s*/
        if(tick500ms_ledState>=2){
            if(distance_mm<8){
            ledState=10;
            }
            else{
            ledState=1;
            }
        }
        break;

        case 10: /*Y blink*/
        LL_GPIO_SetOutputPin(LED_RED_GPIO_Port, LED_RED_Pin);
        LL_GPIO_ResetOutputPin(LED_YELLOW_GPIO_Port, LED_YELLOW_Pin);
        LL_GPIO_SetOutputPin(LED_GREEN_GPIO_Port, LED_GREEN_Pin);
        ledState=11;
        tick500ms_ledState = 0;
        break;
        case 11:/*Y blink*/

        /*servo*/
        servo_pwm_duty0=1100+60*(tick500ms_ledState);/*1100~1700*/
        if(servo_pwm_duty0<=1700){
            LL_TIM_OC_SetCompareCH1(TIM3,servo_pwm_duty0);
        }

        switch(tick500ms_ledState){
        case 1:case 3:case 5:case 7:case 9:
            LL_GPIO_ResetOutputPin(LED_YELLOW_GPIO_Port, LED_YELLOW_Pin);
            break;
        case 2:case 4:case 6:case 8:case 10:
            LL_GPIO_SetOutputPin(LED_YELLOW_GPIO_Port, LED_YELLOW_Pin);
            break;
        case 11:
            ledState=30;
            break;
        default:break;
        }
        break;

        case 30:/*R ON*/
        LL_GPIO_ResetOutputPin(LED_RED_GPIO_Port, LED_RED_Pin);
        LL_GPIO_SetOutputPin(LED_YELLOW_GPIO_Port, LED_YELLOW_Pin);
        LL_GPIO_SetOutputPin(LED_GREEN_GPIO_Port, LED_GREEN_Pin);  
        ledState=31;
        tick500ms_ledState = 0;
        break;
        case 31:/*R ON,state keep at least 2s*/
        if(distance_mm>8){
            if(tick500ms_ledState>4){
            ledState = 0;
            }
        }
        break;
        
        default:
        ledState = 0;
        break;
    }

}